
#pragma once

#include "GLvec3.h"
#include "GLvec4.h"

template<typename T>
class GLmat4
{
public:
	GLvec4<T> x,y,z,w;
public:

	inline T* Base(){
		return &x;
	}

	inline const T* Base() const {
		return &x;
	}

	GLvec4<T>& operator[]( int i ){ return (GLvec4<T>&)((GLvec4<T>*)&x)[i]; }
	const GLvec4<T>& operator[] (int i) const{ return (GLvec4<T>&)((GLvec4<T>*)&x)[i]; }

	void CopyFrom( T *dat){ memcpy( &x,dat,sizeof(T)*16); }
	void CopyTo( T *dat){ memcpy( dat,&x,sizeof(T)*16); }

	void Mult( const GLmat4<T>& rhs )
	{
		int i, j;

		GLmat4<T> t;
		GLmat4<T>a = *this;

		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				t[i][j] = 
				a[i][0]*rhs[0][j] +
				a[i][1]*rhs[1][j] +
				a[i][2]*rhs[2][j] +
				a[i][3]*rhs[3][j];
			}
		}

		CopyFrom( t.Base());
	}

	GLvec3<T>&  operator*( const GLvec3<T>& v )
	{
		return TransformVec3( v );
	}

	GLmat4<T> operator*( const GLmat4<T>& rhs )
	{
		int i, j;

		GLmat4<T> t;

		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				t[i][j] = 
				(*this)[i][0]*rhs[0][j] +
				(*this)[i][1]*rhs[1][j] +
				(*this)[i][2]*rhs[2][j] +
				(*this)[i][3]*rhs[3][j];
			}
		}

		return t;
	}

	GLmat4<T> operator*=( const GLmat4<T>& rhs )
	{
		Mult( rhs );
	}

	void Identity()
	{
		x.x = 1; x.y = 0; x.z = 0; x.w = 0;
		y.x = 0; y.y = 1; y.z = 0; y.w = 0;
		z.x = 0; z.y = 0; z.z = 1; z.w = 0;
		w.x = 0; w.y = 0; w.z = 0; w.w = 1;
	}	
	
	void Transpose()
	{
		Var_ExChange( x.y,y.x );
		Var_ExChange( x.z,z.x );
		Var_ExChange( x.w,w.x );
		Var_ExChange( y.z,z.y );
		Var_ExChange( y.w,w.y );
		Var_ExChange( z.w,w.z );
	}

	GLvec3<T> TransformVec3( const GLvec3<T>& vin )
	{
		GLvec3<T> vout;

		vout[0] = vin[0] * x.x + vin[1] * y.x + vin[2] * z.x + w.x;
		vout[1] = vin[0] * x.y + vin[1] * y.y + vin[2] * z.y + w.y;
		vout[2] = vin[0] * x.z + vin[1] * y.z + vin[2] * z.z + w.z;
		
		return vout;
	}


	bool Invert()
	{
		int i, j, k;
		T t;
		T temp[4][4];

		CopyTo( temp[0] );

		GLmat4 mat_inv;
		mat_inv.Identity();
		
		for (i = 0; i < 4; i++) {

			if (temp[i][i] == 0.0f) {
				for (j = i + 1; j < 4; j++) {
					if (temp[j][i] != 0.0f) {
						break;
					}
				}
			
				if (j != 4) {
			
					for (k = 0; k < 4; k++) {
						t = temp[i][k];
						temp[i][k] = temp[j][k];
						temp[j][k] = t;
				
						t = mat_inv[i][k];
						mat_inv[i][k] = mat_inv[j][k];
						mat_inv[j][k] = t;
					}
				}
				else {
					return false;
				}
			}
			
			t = 1.0f / temp[i][i];
			for (k = 0; k < 4; k++) {
				temp[i][k] *= t;
				mat_inv[i][k] *= t;
			}
			for (j = 0; j < 4; j++) {
				if (j != i) {
					t = temp[j][i];
					for (k = 0; k < 4; k++) {
							temp[j][k] -= temp[i][k]*t;
							mat_inv[j][k] -= mat_inv[i][k]*t;
					}
				}
			}
		}

		CopyFrom( mat_inv.Base() );

		return true;
	}

	void MakeTranslate( const GLvec3<T>& t ){
		Identity();
		w.x = t.x;
		w.y = t.y;
		w.z = t.z;
	}

	void MakeScale( const GLvec3<T>& s ){
		Identity();
		x.x = s.x;
		y.y = s.y;
		z.z = s.z;
	}

	void MakeRotate( T angle1,const GLvec3<T>& axis )
	{
	   T x = axis.x;
	   T y = axis.y;
	   T z = axis.z;

	   T angle = -angle1;

	   T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;
	   bool optimized;

	   s = (GLfloat) sin( angle * DEG2RAD );
	   c = (GLfloat) cos( angle * DEG2RAD );

	   GLmat4 m;

	   m.Identity();
	 
	   optimized = false;


	#define M(row,col)  m[row][col]

	   if (x == 0.0F) {
		  if (y == 0.0F) {
			 if (z != 0.0F) {
				optimized = true;
				/* rotate only around z-axis */
				M(0,0) = c;
				M(1,1) = c;
				if (z < 0.0F) {
				   M(0,1) = s;
				   M(1,0) = -s;
				}
				else {
				   M(0,1) = -s;
				   M(1,0) = s;
				}
			 }
		  }
		  else if (z == 0.0F) {
			 optimized = true;
			 /* rotate only around y-axis */
			 M(0,0) = c;
			 M(2,2) = c;
			 if (y < 0.0F) {
				M(0,2) = -s;
				M(2,0) = s;
			 }
			 else {
				M(0,2) = s;
				M(2,0) = -s;
			 }
		  }
	   }
	   else if (y == 0.0F) {
		  if (z == 0.0F) {
			 optimized = true;
			 /* rotate only around x-axis */
			 M(1,1) = c;
			 M(2,2) = c;
			 if (x < 0.0F) {
				M(1,2) = s;
				M(2,1) = -s;
			 }
			 else {
				M(1,2) = -s;
				M(2,1) = s;
			 }
		  }
	   }

	   if (!optimized) {
		  const GLfloat mag = sqrt(x * x + y * y + z * z);

		  if (mag <= 1.0e-4) {
			 /* no rotation, leave mat as-is */
			 return;
		  }

		  x /= mag;
		  y /= mag;
		  z /= mag;


		  /*
		   *     Arbitrary axis rotation matrix.
		   *
		   *  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
		   *  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
		   *  (which is about the X-axis), and the two composite transforms
		   *  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
		   *  from the arbitrary axis to the X-axis then back.  They are
		   *  all elementary rotations.
		   *
		   *  Rz' is a rotation about the Z-axis, to bring the axis vector
		   *  into the x-z plane.  Then Ry' is applied, rotating about the
		   *  Y-axis to bring the axis vector parallel with the X-axis.  The
		   *  rotation about the X-axis is then performed.  Ry and Rz are
		   *  simply the respective Invert transforms to bring the arbitrary
		   *  axis back to it's original orientation.  The first transforms
		   *  Rz' and Ry' are considered Inverts, since the data from the
		   *  arbitrary axis gives you info on how to get to it, not how
		   *  to get away from it, and an Invert must be applied.
		   *
		   *  The basic calculation used is to recognize that the arbitrary
		   *  axis vector (x, y, z), since it is of unit length, actually
		   *  represents the sines and cosines of the angles to rotate the
		   *  X-axis to the same orientation, with theta being the angle about
		   *  Z and phi the angle about Y (in the order described above)
		   *  as follows:
		   *
		   *  cos ( theta ) = x / sqrt ( 1 - z^2 )
		   *  sin ( theta ) = y / sqrt ( 1 - z^2 )
		   *
		   *  cos ( phi ) = sqrt ( 1 - z^2 )
		   *  sin ( phi ) = z
		   *
		   *  Note that cos ( phi ) can further be inserted to the above
		   *  formulas:
		   *
		   *  cos ( theta ) = x / cos ( phi )
		   *  sin ( theta ) = y / sin ( phi )
		   *
		   *  ...etc.  Because of those relations and the standard trigonometric
		   *  relations, it is pssible to reduce the transforms down to what
		   *  is used below.  It may be that any primary axis chosen will give the
		   *  same results (modulo a sign convention) using thie method.
		   *
		   *  Particularly nice is to notice that all divisions that might
		   *  have caused trouble when parallel to certain planes or
		   *  axis go away with care paid to reducing the expressions.
		   *  After checking, it does perform correctly under all cases, since
		   *  in all the cases of division where the denominator would have
		   *  been zero, the numerator would have been zero as well, giving
		   *  the expected result.
		   */

		  xx = x * x;
		  yy = y * y;
		  zz = z * z;
		  xy = x * y;
		  yz = y * z;
		  zx = z * x;
		  xs = x * s;
		  ys = y * s;
		  zs = z * s;
		  one_c = 1.0F - c;

		  /* We already hold the identity-matrix so we can skip some statements */
		  M(0,0) = (one_c * xx) + c;
		  M(0,1) = (one_c * xy) - zs;
		  M(0,2) = (one_c * zx) + ys;
	/*    M(0,3) = 0.0F; */

		  M(1,0) = (one_c * xy) + zs;
		  M(1,1) = (one_c * yy) + c;
		  M(1,2) = (one_c * yz) - xs;
	/*    M(1,3) = 0.0F; */

		  M(2,0) = (one_c * zx) - ys;
		  M(2,1) = (one_c * yz) + xs;
		  M(2,2) = (one_c * zz) + c;
	/*    M(2,3) = 0.0F; */

	/*
		  M(3,0) = 0.0F;
		  M(3,1) = 0.0F;
		  M(3,2) = 0.0F;
		  M(3,3) = 1.0F;
	*/
	   }
	#undef M

	   CopyFrom( m.Base());
	}

	void MakeRotateX( T angle ){ MakeRotate(angle,GLvec3<T>(1,0,0));}
	void MakeRotateY( T angle ){ MakeRotate(angle,GLvec3<T>(0,1,0));}
	void MakeRotateZ( T angle ){ MakeRotate(angle,GLvec3<T>(0,0,1));}

	void MakeRotate( GLvec3<T> axes ){
		GLmat4 m1,m2,m3,m;
		m1.MakeRotateX( axes.x );
		m2.MakeRotateY( axes.y );
		m3.MakeRotateZ( axes.z );
		m = m1*m2;
		m = m*m3;
		CopyFrom( m.Base());
	}

	// 1		0			0
	//	0	cos(rx)		sin(rx)
	//	0	-sin(rx)	cos(rx)
	void RotationX(  float Angle )
	{
		GLmat4 mout;
		mout.Identity();

		mout[1][1] = mout[2][2] = (float)cos(Angle);
		mout[1][2] = (float)sin(Angle);
		mout[2][1] = -mout[1][2];

		CopyFrom(mout.Base());
	}

	// cos(ry)		0		-sin(ry)
	//	0			1			0
	// sin(ry)		0		cos(ry)
	void RotationY(   float Angle )
	{
		GLmat4 mout;
		mout.Identity ();

		mout[0][0] = mout[2][2] = (float)cos(Angle);
		mout[2][0] = (float)sin(Angle);
		mout[0][2] = -mout[2][0];
		CopyFrom(mout.Base());

	}

	// cos(rz)		sin(rz)		0
	// -sin(rz)		cos(rz)		0
	// 0				0		1
	void RotationZ(  float Angle )
	{
		GLmat4 mout; 
		mout.Identity ();

		mout[0][0] = mout[1][1] = (float)cos(Angle);
		mout[0][1] = (float)sin(Angle);
		mout[1][0] = -mout[0][1];
		CopyFrom(mout.Base());
	}

	void Compose( GLvec3<T> t,GLvec3<T> r,GLvec3<T> s )
	{
		// Ordering:
		//    1. Scale
		//    2. Rotate
		//    3. Translate

		GLmat4<T> matScale, matRotate, matTranslate;
		GLmat4<T> m1, m2,m3;

		matScale.MakeScale( s );
		//matRotate.MakeRotate( r );
		
		m1.RotationX(r.x);
		m2.RotationY(r.y);
		m3.RotationZ(r.z);

		matRotate = m1*m2;
		matRotate = matRotate*m3;

		matTranslate.MakeTranslate( t );

		GLmat4<T> m;

		m = matScale*matRotate;
		m = m*matTranslate;

		CopyFrom( m.Base());

	}

	GLmat4<T>& operator=(const GLmat4<T>& rhs){x = rhs.x;y = rhs.y;z=rhs.z;w=rhs.w;return *this;}
	GLmat4<T>& operator=(T* m){ CopyFrom( m );}

};
typedef GLmat4< int > GLmat4i;
typedef GLmat4< float > GLmat4f;
typedef GLmat4< double > GLmat4d;
